% ************************************************************************ 
%     Module name   : Fusion Simulation - Predict Intercepts                                                     
%                                                                          
%     Description   : This will predict the sequence of when the radars will intercept the targets in the scene.                                                    
%                                                                          
%     $Header: /Lynx/SimulationFramework/RSP/IFS/IFSRun.m 2     15/10/08 16:08 Lwabeke $ 
%                                                                          
%    $Author: Lwabeke $	            
%                                                                         
%    $Revision: 2 $                
%                                                                         
% ************************************************************************
%     $History: IFSRun.m $ 
% 
%
% ************************************************************************

function [TargetSequence, TargetAngles, TargetSequenceBinned] = predictIntercepts(currentTime, radarState, param)

global worldState;

activeTargets = 0;

intercept_angles = zeros(worldState.activeTargets, param.num_radars);

for loopCntr=1:2
    
    if (loopCntr==1)
        startCntr=1;
        stopCntr=worldState.numTargets;
    else
        startCntr=worldState.oldestActive;
    end
    
    tgtCntr = startCntr;
    
    while ((loopCntr == 1)&&(tgtCntr <= stopCntr)) ...
            ||((loopCntr == 2)&&(tgtCntr < size(worldState.Target,2))&&(worldState.Target(tgtCntr).initTime < currentTime))
        
        Target = worldState.Target(tgtCntr);
        
        if (currentTime < Target.initTime)
            % Not visible yet, only useful for targets, clutter sorting and
            % sliding window should handle this case
        elseif (currentTime < Target.endTime)
            if ((loopCntr == 2)&&(worldState.oldestActive == 0))
                oldestActive = tgtCntr;   % Keeps track of the oldest clutter index that was found to still be active
            end
            
            %count active targets
            activeTargets = activeTargets +1;
            
            for countRadars = 1:param.num_radars
                delta_pos = Target.currentPos - param.radars(countRadars).location;
                intercept_angle = atan2(delta_pos(2), delta_pos(1)) - radarState.radar(countRadars).posAngle;
                intercept_angles(activeTargets, countRadars) = floor(intercept_angle / param.radarAngleResolution) * param.radarAngleResolution;
            end
            
        elseif (Target.endTime < currentTime)
            worldState.Target(tgtCntr).currentPos = Target.endPos;
        end
        
       
        tgtCntr = tgtCntr + 1;
    end
    
end


[sorted, sort_indexes] = sort(abs(intercept_angles));

TargetSequence = sort_indexes;
TargetAngles = intercept_angles;

for radarCnt = 1:param.num_radars
    unique_elements = unique(sorted(:,radarCnt));
    for cntr = 1:size(unique_elements)
        match = (sorted(:,radarCnt) == unique_elements(cntr));
%         if (sum(match) > 1)
%            match 
%         end
        TargetSequenceBinned.radar(radarCnt).TargetSequence(cntr,1:sum(match)) = sort_indexes(match,radarCnt).';
    end
end


